//----------------------------------------------------------------------------
// Filename                : commutator.c
//
// Functional Description  : Commutator support functions
//----------------------------------------------------------------------------

#include "DRV922xx_trapezoidal_sensored.h"
#include "message_protocol_application.h"

//
// HALL SENSORS:
//
//  HS1: P3.1 (U)
//  HS2: P3.2 (V)
//  HS3: P3.3 (W)
 
//
// unsigned char Comm_Values[]
//
// Contains set of  values used for P2SEL and P2OUT for motor commutation for both forward and reverse, non-complementary and 
// complementary modes.
// 
// The hall sensor values are used as an index into this table, so that the values for P2OUT (IO state) and P2SEL (PWM or non-PWM mode) 
// can be quickly fetched and assigned.
//
// Note that this table is indexed from 0 to 5, where each row contains 8 bytes (for ease of access - power of 2) of data for
// each communtation state. The index into this table is:
//
//   row_index = (P3IN >> 1) - 1;	// shift right bits to align and subtract one that will give index from 0 - 5
//   array_index = row_index << 3;	// get first byte of the correct row 
//
// Each row in this table contains the following bytes:
//   Index 0: Value of P2OUT (forward drive)
//   Index 1: Value of P2SEL (non-complementary, forward drive)
//   Index 2: Value of P2SEL (complementary, forward drive)
//   Index 3: Value of P2OUT (reverse drive)
//	 Index 4: Value of P2SEL (non-complementary, reverse drive)
//	 Index 5: Value of P2SEL (complementary, reverse drive)
//   Index 6-7: 0xFF - dummy for 8 byte rows
//
// The commutation order for the GWheel is as follows:
//
//   Forward:  1 >> 2 >> 0 >> 4 >> 3 >> 5 >> 1 >> ...
//   Reverse:  1 >> 5 >> 3 >> 4 >> 0 >> 2 >> 1 >> ...
//

const unsigned char Comm_Values[] = 
{
	0x10, 0x02,	0x06, 0x04, 0x08, 0x18, 0xFF, 0xFF,
	
						// Commutation index 0 (001b HALL):
						//  
						// Forward Drive:
						// =============
						// U: 		High [UHDR = PWM   , ULDR = OFF/~PWM] 
						// V: 		Low  [VHDR = OFF   , VLDR = ON]
	                	// W: 		Hi-Z [WHDR = OFF   , WLDR = OFF]
	                	// P2OUT:	0x10
	                	// P2SEL:	0x02/0x06
	                	//
	                	// Reverse Drive:
	                	// =============
	                	// U:		Low  [UHDR = OFF   , ULDR = ON]
	                	// V:		High [VHDR = PWM   , VHDR = OFF/~PWM
	                	// W:		Hi-Z [WHDR = OFF   , WLDR = OFF]
	                	// P2OUT:   0x04
	                	// P2SEL:   0x80/0x18
	                
	0x40, 0x08,	0x18, 0x10, 0x20, 0x60, 0xFF, 0xFF,	
	
						// Commutation index 1 (010b HALL):
						//  
						// Forward Drive:
						// =============
						// U: 		Hi-Z [UHDR = OFF   , ULDR = OFF]
	                	// V: 		High [VHDR = PWM   , VLDR = OFF/~PWM]
	                	// W: 		Low  [WHDR = OFF   , WLDR = ON]
	                	// P2OUT:	0x40
	                	// P2SEL:	0x08/0x18
	                	//
	                	// Reverse Drive:
	                	// =============
	                	// U:		Hi-Z [UHDR = OFF   , ULDR = OFF]
	                	// V:		Low  [VHDR = OFF   , VLDR = ON]
	                	// W: 		High [WHDR = PWM   , WLDR = OFF/~PWM]
	                	// P2OUT:	0x10
	                	// P2SEL:	0x20/0x60
	                	
	0x40, 0x02, 0x06, 0x04, 0x20, 0x60, 0xFF, 0xFF,
	
						// Commutation index 2 (011b HALL):
						//  
						// Forward Drive:
						// =============
						// U: 		High [UHDR = PWM   , ULDR = OFF/~PWM]
	                	// V: 		Hi-Z [VHDR = OFF   , VLDR = OFF]
	                	// W: 		Low  [WHDR = OFF   , WLDR = ON]
	                	// P2OUT:	0x40
	                	// P2SEL:	0x02/0x06
	                	//
	                	// Reverse Drive:
	                	// =============
	                	// U: 		Low  [UHDR = OFF   , ULDR = ON]
	                	// V:		Hi-Z [VHDR = OFF   , VLDR = OFF]
	                	// W:		High [WHDR = PWM   , WLDR = OFF/~PWM]
	                	// P2OUT:	0x04
	                	// P2SEL:	0x20/0x60

	0x04, 0x20,	0x60, 0x40, 0x02, 0x06, 0xFF, 0xFF,
	
						// Commutation index 3 (100b HALL):
						//  
						// Forward Drive:
						// =============
						// U:		Low  [UHDR = OFF   , ULDR = ON]
	                	// V: 		Hi-Z [VHDR = OFF   , VLDR = OFF]
	                	// W: 		High [WHDR = PWM   , WLDR = OFF/~PWM]
	                	// P2OUT:	0x04
	                	// P2SEL:	0x20/0x60
	                	//
	                	// Reverse Drive:
	                	// =============
	                	// U:		High [UHDR = PWM   , ULDR = OFF/~PWM]
	                	// V:		Hi-Z [VHDR = OFF   , VLDR = OFF]
	                	// W:		Low  [WHDR = OFF   , WLDR = ON]
	                	// P2OUT:	0x40
	                	// P2SEL:	0x02/0x06
	                	
	0x10, 0x20,	0x60, 0x40, 0x08, 0x18, 0xFF, 0xFF,
	
						// Commutation index 4 (101b HALL):
						//  
						// Forward Drive:
						// =============
						// U: 		Hi-Z [UHDR = OFF   , ULDR = OFF]
	                	// V: 		Low  [VHDR = OFF   , VLDR = ON]
	                	// W: 		High [WHDR = PWM   , WLDR = OFF/~PWM]
	                	// P2OUT:	0x10
	                	// P2SEL:	0x20/0x60
	                	//
	                	// Reverse Drive:
	                	// =============
	                	// U:		Hi-Z [UHDR = OFF   , ULDR = OFF]
	                	// V:		High [VHDR = PWM   , VLDR = OFF/~PWM]
	                	// W:		Low  [WHDR = OFF   , WLDR = ON]
	                	// P2OUT:	0x40
	                	// P2SEL:	0x08/0x18
	                	
	0x04, 0x08, 0x18, 0x10, 0x02, 0x06, 0xFF, 0xFF
	
						// Commutation index 5 (110b HALL):
						//  
						// Forward Drive:
						// =============
						// U:		Low  [UHDR = OFF   , ULDR = ON]
	                	// V:		High [VHDR = PWM   , ULDR = OFF/~PWM]
	                	// W:		Hi-Z [WHDR = OFF   , WLDR = OFF]
	                	// P2OUT:	0x04
	                	// P2SEL:	0x08/0x18
	                	//
	                	// Reverse Drive:
	                	// =============
	                	// U:		High [UHDR = PWM   , ULDR = OFF/~PWM]
	                	// V:		Low  [VHDR = OFF   , VLDR = ON]
	                	// W:       Hi-Z [WHDR = OFF   , WLDR = OFF]
	                	// P2OUT: 	0x10
	                	// P2SEL:	0x02/0x06
};

/*
 *  \fn void update_com()
 *  \brief Updates high and low-side driver gates/PWMs depending on commutator states
 * 
 *  The PWM units are aleays running to make sure that they stay in sync.
 *  They are enbled at the port (P2SEL) when a commutation is needed.
 *  All constant values are set uing the ports (P2.6-1)
 */
void update_com(void)
{
	static unsigned int pwmTicksLast = 0;
	unsigned int pwmTicksTotal = 0;
	unsigned int commTime = 0;
//	pwm_blink_led();
//	update_commutator = 0;
//	char new_com_state;
//	P1OUT |= BIT5;   
	pwmTicks = 10000;
	pwmTicksLast = 4000;
	    
	if (pwmTicks > pwmTicksLast)
	{
		#define MICROSEC_PER_SEC 1000000
		#define NUM_POLE_PAIRS_PER_REV		23
		#define NUM_COMMUTATIONS_PER_POLE_PAIR	6
		#define NUM_SEC_PER_MIN		60
		pwmTicksTotal = pwmTicks - pwmTicksLast;
		commTime = pwmTicksTotal * 624 / 10;
		speed = (MICROSEC_PER_SEC * NUM_SEC_PER_MIN ) / (commTime * NUM_COMMUTATIONS_PER_POLE_PAIR * NUM_POLE_PAIRS_PER_REV);
		speedFiltered = (speed>>2) + 3*(speedFiltered>>2);
	}    
    new_com_state = (P3IN & 0x0e) >> 1; // get new state
    
//    if ((new_com_state != saved_com_state))         	
   		saved_com_state = new_com_state;  // capture new_com_state for future comparison
            
        // Set Interrupt edge, turns out its the same as the current state
        P3IES = new_com_state << 1;
    if ((new_com_state == 0)||(new_com_state == 0x7))
    {
    	pwm_disable();
    	return;
    }
 
	    comm_tbl_index = (new_com_state - 1) << 3;				// 8 entries per com state, plus starts at index - 1 (0-5 rather than 1-6)
	           
	    // See if we should commute the wheel backward or forward.
		// If P4.6 is high, then we are in reverse drive mode, if high then we are in forward drive mode
	
	// Change commutator
	if ((just_changed_direction == 0)&&(mode == MODE_ON))
	{
		// Make sure to disable interrupts during this time
//		update_commutator = 1;
		pwm_enable(comm_tbl_index);
		timeOffSec = 0;
	}

//		else if (just_changed_direction == 1) {//(++timeOffSec > 10000) {
//			timeOffSec = 0;
 //   		just_changed_direction = 0;
  //  		if (target_drive_direction == DIR_REVERSE)
 //   			reverse_drive = 1;
 //   		else if (target_drive_direction == DIR_FORWARD)
 //   			reverse_drive = 0;
 //   		AppStatus &= ~StatusBemf;
 //   		enter_align_go_mode();
//		}
//		else {
//			send_cmd(CmdStatus, timeOffSec);
//		}

//    }  // end of if (saved_comm_state)
//	P1OUT &= ~BIT5;
	pwmTicksLast = pwmTicks;
}
